#include "toalocalization.h"
#include <QRandomGenerator>
#include <numeric>
#include "websocket.h"
#include"mainwindow.h"

// extern QPoint* p1;
// extern QPoint* p2;
// extern QPoint* p3;
TOALocalization::TOALocalization(QObject *parent) : QObject(parent)
{

    // 参数初始化
    n = 100;    // 最大迭代次数
    m = 800;    // 坐标点数量

    // 基站位置初始化 (单位: cm)
    Si = {
        QPointF(8471.087454448428, 276.24456077825744),
        QPointF(8423.40046065205,226.9013223137008),
        QPointF(10403.82080078125,22823.379516601562)
        // *p1,
        // *p2,
        // *p3
    };
    M = Si.size();  // 基站数量

    // 初始化矩阵 - 确保所有维度正确设置
    ni.resize(M); //调整矩阵为M列
    ri.resize(M);
    for (auto &row : ri) {
        row.resize(m);
/*原型函数  template<class ForwardIt, class T>
           void fill(ForwardIt first, ForwardIt last, const T& value);
  作用​：将 [first, last) 区间内的所有元素设置为 value
*/
        std::fill(row.begin(), row.end(), 0.0);  // 初始化为0
    }

    C.resize(2);
    for (auto &row : C) {
        row.resize(n);
        std::fill(row.begin(), row.end(), 0.0);
    }

    U.resize(2);
    for (auto &row : U) {
        row.resize(m);
        std::fill(row.begin(), row.end(), 0.0);
    }

    ei.resize(m);
    for (auto &row : ei) {
        row.resize(n);
        std::fill(row.begin(), row.end(), 0.0);
    }

    ek.resize(n);
    for (auto &row : ek) {
        row.resize(M);
        std::fill(row.begin(), row.end(), 0.0);
    }

    u0.resize(2);
    for (auto &row : u0) {
        row.resize(m);
        std::fill(row.begin(), row.end(), 0.0);
    }

    // 初始迭代质心位置设为基站质心
    double sumX = 0.0, sumY = 0.0;
    for (const auto &point : Si) {
        sumX += point.x();
        sumY += point.y();
    }
    // 存储质心
    C[0][0] = sumX / M;
    C[1][0] = sumY / M;

    // 设置初始迭代误差为0
    for (int j = 0; j < M; ++j) {
        ek[0][j] = 0.0;
    }
}

// 暂时没起作用
void TOALocalization::startCalculation()
{
    currentPoint = 0;

    // 重置所有矩阵
    for (auto &row : ri) {
        std::fill(row.begin(), row.end(), 0.0);
    }
    for (auto &row : ei) {
        std::fill(row.begin(), row.end(), 0.0);
    }
    for (auto &row : U) {
        std::fill(row.begin(), row.end(), 0.0);
    }

}

bool TOALocalization::calculateNextPoint(QString id)
{
    if (currentPoint >= m) return false;

    calculatePoint(currentPoint,id);
  //  emit pointCalculated(currentPoint, U[0][currentPoint], U[1][currentPoint]); // 发送坐标

    currentPoint++;
    return currentPoint < m;
}

void TOALocalization::calculatePoint(int i,QString id)
{

    // 添加边界检查
    if (i < 0 || i >= m) {
        qDebug() << "Invalid point index:" << i;
        return;
    }

    // 初始迭代质心位置设为基站质心
    double sumX = 0.0, sumY = 0.0;
    for (const auto &point : Si) {
        sumX += point.x();
        sumY += point.y();
    }
    C[0][0] = sumX / M;
    C[1][0] = sumY / M;

    // 迭代过程
    int k;
    for (k = 1; k < n; ++k) {
        QVector<double> tempSum1(M, 0.0);
        QVector<double> tempSum2(M, 0.0);

        for (int j = 0; j < M; ++j) {
            // 添加边界检查
            if (j >= Si.size() || k-1 >= C[0].size()) {
                continue;
            }

            double dx = C[0][k-1] - Si[j].x();
            double dy = C[1][k-1] - Si[j].y();
            double normVal = qSqrt(dx*dx + dy*dy);

            if (normVal > 0) {  // 避免除以0
                tempSum1[j] = ri[j][i] * dx / normVal + Si[j].x();
                tempSum2[j] = ri[j][i] * dy / normVal + Si[j].y();
            }
        }

        if (k < C[0].size()) {
            C[0][k] = std::accumulate(tempSum1.begin(), tempSum1.end(), 0.0) / M;
            C[1][k] = std::accumulate(tempSum2.begin(), tempSum2.end(), 0.0) / M;
        }

        // 计算迭代误差
        for (int a = 0; a < M; ++a) {
            if (a >= Si.size() || k >= ek.size() || a >= ek[k].size()) {
                continue;
            }

            double dx = C[0][k] - Si[a].x();
            double dy = C[1][k] - Si[a].y();
            double dist = qSqrt(dx*dx + dy*dy);

            if (ri[a][i] > 0) {  // 避免除以0
                ek[k][a] = (ri[a][i] - dist) / ri[a][i];
            } else {
                ek[k][a] = 0.0;
            }
        }

        // 计算误差变化
        double errorSum = 0.0;
        for (int a = 0; a < M; ++a) {
            if (a >= ek[k].size() || a >= ek[k-1].size()) {
                continue;
            }
            errorSum += qPow(ek[k][a] - ek[k-1][a], 2);
        }

        if (i < ei.size() && k < ei[i].size()) {
            ei[i][k] = qSqrt(errorSum);
        }

        if (i < ei.size() && k < ei[i].size() && ei[i][k] <= 0.001) {
            break;
        }
    }

    // 存储最终解算出的坐标点
    if (i < U[0].size()) {
        U[0][i] = C[0][k];
        U[1][i] = C[1][k];
    }

    emit pointCalculated(id, U[0][currentPoint], U[1][currentPoint]); // 发送坐标
    //qDebug() << "即将发送坐标:" << U[0][currentPoint] << U[1][currentPoint];
    //WebSocket ws;
    //if(ws.number>0){
    //    ws.sendLocation(U[0][currentPoint], U[1][currentPoint]);
    //}
    QVector<double> start={U[0][currentPoint]/100,0, U[1][currentPoint]/100};
    fm.finalPath(start,start);
    qDebug()<<"3";
}

void TOALocalization::resetCalculation()
{
    currentPoint = 0;
}

void TOALocalization::updateMeasurements(const QVector<double> &distances)
{
    // 添加边界检查
    if (currentPoint >= m) {
        qDebug() << "currentPoint out of range:" << currentPoint << ">=" << m;
        return;
    }

    if (distances.size() != M) {
        qDebug() << "Invalid distances size:" << distances.size() << "!=" << M;
        return;
    }

    // 确保 ri 有足够的空间
    if (ri.size() < M) {
        ri.resize(M);
    }

    for (auto &row : ri) {
        if (row.size() <= currentPoint) {
            row.resize(m);  // 直接一次性分配足够空间
        }
    }

    // 更新测量值
    for (int j = 0; j < M; ++j) {
        if (j < distances.size()) {
            ri[j][currentPoint] = distances[j] * 100; // 转换为厘米
            qDebug() <<  ri[j][currentPoint];
        } else {
            ri[j][currentPoint] = 0.0;  // 默认值
        }

    }

   // calculateNextPoint();

}
